/***************************************************************************
 创建者: 华磊
 开始时间: 2019.4.3
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file
 *  功能:
 * 伺服位置环
 *
 *                                                                         *
 ***************************************************************************/
#ifndef POSITIONREGULATORGROUP_H
#define POSITIONREGULATORGROUP_H
#include <QMap>
#include "GeneralDefine.h"
#include "mathonlydefine.h"
#include <QMutex>

class PositionRegulator;
class MotionMessage;

class PositionRegulatorGroup
{
public:
    PositionRegulatorGroup(QString filePathIn, double deltaTimeIn, MotionMessage *motionMessageIn, int robotIdIn,E_REGULATOR_TYPE typeIn);

public:
    int getPositionRegulatorDebugInfo(int regulatorIndex,RegulatorDebugInfo& infoOut);
    int setPositionInput(QVector<double> positionInput);
    int setServoStatus(QVector<bool> isServoOn);
    int loopRegulatorOnce_withPosition(QVector<double> feedforwardPosition,QVector<double> targetPosition,
                                       QVector<double> feedbackPosition,QVector<double> &velOutut);
    int loopRegulatorOnce(QVector<double> targetPosition,QVector<double> feedbackPosition,QVector<double> &velOutut);
    int loopRegulatorOnce(QVector<double> feedbackPosition, QVector<double> &velOutut);
    int isValid( QVector<bool> &isValidOut);
    bool isInitialOkStatus();
    int setPositionPid(int axisIdIn,double pid_p, double pid_i, double pid_d, double errorLimitForIntegration,double feedforward);
    int quickStop();
    int stop();
    int cease();
    int resetAll();
    int startRun();

private:
    int readConfigFile(QString filePathIn);
    void addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode
                , int _robotId, int parameter1=0,int parameter2=0);

private:
    QMap<int,PositionRegulator*> regulatorList;
    QMap<int,RegulatorConfigInfo> configInfos;
    bool isInitialOk;
    MotionMessage *motionMessage;
    int robotId;
    double deltaTime;
    QMutex usingMutex;
    E_REGULATOR_TYPE regulatorType;

};

#endif // POSITIONREGULATORGROUP_H
